#include "vel_pose_connector_core.h"

VelPoseConnector::VelPoseConnector():nh_(""), private_nh_("~"), listener_(buffer_){
    private_nh_.param<std::string>("output_pose_topic_", output_pose_topic_, "");
    private_nh_.param<std::string>("output_velocity_topic_", output_velocity_topic_, "");
    private_nh_.param<std::string>("output_odom_topic_", output_odom_topic_, "");
}

VelPoseConnector::~VelPoseConnector(){}

void VelPoseConnector::run(){
    pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(output_pose_topic_, 1);
    velocity_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(output_velocity_topic_, 1);
    odom_pub_ = nh_.advertise<nav_msgs::Odometry>(output_odom_topic_, 1);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &VelPoseConnector::process, this, false, true);
    ros::spin();
}

double VelPoseConnector::delta_yaw(const double &orin_yaw, const double &yaw){
    std::vector<double> vector_orin_yaw;
    std::vector<double> vector_yaw;
    vector_orin_yaw.push_back(sin(orin_yaw));
    vector_orin_yaw.push_back(cos(orin_yaw));
    vector_yaw.push_back(sin(yaw));
    vector_yaw.push_back(cos(yaw));
    double dot = vector_yaw[0]*vector_orin_yaw[0]+vector_yaw[1]*vector_orin_yaw[1];
    double cross = vector_yaw[0]*vector_orin_yaw[1]-vector_yaw[1]*vector_orin_yaw[0];
    double delta = acos(fabs(dot));
    if(cross<=0.0){
        delta = -1.0*delta;
    }
    return delta;
}

void VelPoseConnector::process(const ros::TimerEvent& e){
    try{
        /* 发布定位信息 */
        geometry_msgs::TransformStamped tf = buffer_.lookupTransform("map", "base_link", ros::Time().now(), ros::Duration(0.5));
        // ROS_INFO("[%s] get dynamic tf successfully", __APP_NAME__);
        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = "map";
        pose.header.stamp = ros::Time().now();
        pose.pose.position.x = tf.transform.translation.x;
        pose.pose.position.y = tf.transform.translation.y;
        pose.pose.position.z = tf.transform.translation.z;
        pose.pose.orientation.x = tf.transform.rotation.x;
        pose.pose.orientation.y = tf.transform.rotation.y;
        pose.pose.orientation.z = tf.transform.rotation.z;
        pose.pose.orientation.w = tf.transform.rotation.w;
        pose_pub_.publish(pose);

        /* 发布odom信息 */
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = ros::Time().now();
        odom_msg.header.frame_id = "map";
        odom_msg.child_frame_id = "base_link";
        odom_msg.pose.pose.position.x = tf.transform.translation.x;
        odom_msg.pose.pose.position.y = tf.transform.translation.y;
        odom_msg.pose.pose.position.z = tf.transform.translation.z;
        odom_msg.pose.pose.orientation.x = tf.transform.rotation.x;
        odom_msg.pose.pose.orientation.y = tf.transform.rotation.y;
        odom_msg.pose.pose.orientation.z = tf.transform.rotation.z;
        odom_msg.pose.pose.orientation.w = tf.transform.rotation.w;
        odom_pub_.publish(odom_msg);

        /* 发布速度信息 */
        if(ref_pose_ == nullptr){
            ref_pose_ = std::make_shared<geometry_msgs::PoseStamped>(pose);
        }else{
            ts_.header.frame_id = "base_link";
            ts_.header.stamp = ros::Time().now();
            Eigen::Vector2d trans_vector = {pose.pose.position.x-ref_pose_->pose.position.x, 
                                            pose.pose.position.y-ref_pose_->pose.position.y};

            double roll, pitch, yaw, roll_ref, pitch_ref, yaw_ref;
            tf::Quaternion quat;
            tf::quaternionMsgToTF(pose.pose.orientation, quat);
            tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
            tf::quaternionMsgToTF(ref_pose_->pose.orientation, quat);
            tf::Matrix3x3(quat).getRPY(roll_ref, pitch_ref, yaw_ref);

            double delta_time = ros::Time().now().toSec()-ref_pose_->header.stamp.toSec();
            double trans = sqrt(trans_vector.squaredNorm());
            trans_vector.normalize();
            if(delta_time>=0.1){
                Eigen::Vector2d vector_vehicle = {cos(yaw), sin(yaw)};
                double vx, angular;
                vx = vector_vehicle[0]*trans_vector[0]+vector_vehicle[1]*trans_vector[1]>0?trans/delta_time:-1.0*trans/delta_time;
                angular = delta_yaw(yaw_ref, yaw)/delta_time;
                ts_.twist.linear.x = vx;
                ts_.twist.angular.z = angular;
                ref_pose_ = std::make_shared<geometry_msgs::PoseStamped>(pose);
            }
        }
    }catch (tf2::TransformException &ex)
    {   
        ROS_ERROR("[%s] localization error......", __APP_NAME__);
    }
}
